#include "GeoCoord.h"
#include "RefFrame.h"

namespace uav
{
namespace nav
{

Vector3 GeoCoord::toEcef() const
{
	Fixed sinLat, cosLat, sinLon, cosLon;
	latitude.sinCos(sinLat, cosLat);
	longitude.sinCos(sinLon, cosLon);

	Fixed c = sqrt(Fixed::One - WGS84_E2 * (sinLat * sinLat));
	Fixed ac = WGS84_A / c;
	Fixed temp = (ac + altitude) * cosLat;

	return Vector3(
		temp * cosLon,
		temp * sinLon,
		(ac * WGS84_1_MINUS_E2 + altitude) * sinLat
	);
}

Vector3 GeoCoord::toEnu(const RefFrame &ref) const
{
	Vector3 temp = toEcef();
	temp -= ref.originEcef;
	return Vector3::transform(ref.ecefToLocal, temp);
}

}}
